#include <Arduino.h>
#include <U8g2lib.h>
#ifdef U8X8_HAVE_HW_SPI
#include <SPI.h>
#endif
#ifdef U8X8_HAVE_HW_I2C
#include <Wire.h>
#endif
#define U8X8_HAVE_HW_I2C
U8G2_SSD1306_128X64_NONAME_F_SW_I2C u8g2(U8G2_R0, /* clock=*/SCL, /* data=*/SDA, /* reset=*/U8X8_PIN_NONE); // All Boards without Reset of the Display

#include "painlessMesh.h"
#define MESH_PREFIX "whateverYouLike"
#define MESH_PASSWORD "somethingSneaky"
#define MESH_PORT 5555

Scheduler userScheduler; // to control your personal task
painlessMesh mesh;
SimpleList<uint32_t> nodes;

bool turnSTB;
float heading = 0;
// User stub
void sendMessage(); // Prototype so PlatformIO doesn't complain

// Task taskSendMessage(TASK_SECOND * 1, TASK_FOREVER, &sendMessage);

void sendMessage(String msg)
{
    msg += mesh.getNodeId();
    mesh.sendBroadcast(msg);
    Serial.printf("T:%s\n", msg.c_str());
}

// Needed for painless library
void receivedCallback(uint32_t from, String &msg)
{

    float headingNow;
    char lcdBuf[20], talker[10], rot;
    digitalWrite(2, 0);
    u8g2.setFont(u8g2_font_inr27_mf);

    Serial.printf("R[%u]:%s\n", from, msg.c_str());
    if (sscanf(msg.c_str(), "ROT:%c", &rot))
    {
        if (rot == 'P')
            u8g2.drawUTF8(0, 65, "<----");
        if (rot == 'S')
            u8g2.drawUTF8(0, 65, "---->");
    }
    if (sscanf(msg.c_str(), "HDT:%s", lcdBuf))
    {
        //    u8g2.clearBuffer();
        sprintf(lcdBuf, "%s°", lcdBuf);
        u8g2.drawUTF8(0, 30, lcdBuf);
    }
    u8g2.sendBuffer();
    if (strcmp(msg.c_str(), "test") == 0)
        mesh.sendBroadcast("onLine");
    digitalWrite(2, 1); // blinking();
}
void lcdShow(String &msg)
{
    char buf[50];
    int len = strlen(msg.c_str());
    u8g2.clearBuffer();
    for (int i = 0; i < len; i++)
    {
        buf[i % 16] = msg[i];
        if (i % 16 == 15)
        {
            buf[i % 16 + 1] = '\0';
            u8g2.drawUTF8(0, (i / 15) * 15, buf);
            //		Serial.printf("%d:%s", (i / 15) * 15, buf);
        }
    }
    buf[len % 16 + 1] = '\0';
    u8g2.drawUTF8(0, (len / 15 + 1) * 15, buf);
    //	Serial.printf("%d:%s", (len / 15) * 15, buf);
    // u8g2.drawUTF8(0, 15, msg.c_str());
    u8g2.sendBuffer();
}
void newConnectionCallback(uint32_t nodeId)
{
    char msg[50], buf[50];
    int len = sprintf(msg, "New Connection, nodeId = %u\n", nodeId);
    Serial.printf(msg);
}

void changedConnectionCallback()
{
    Serial.printf("Changed connections\n");
    u8g2.clearBuffer();
    char buf[20];
    int i = 2;
    // Show initial text
    nodes = mesh.getNodeList();
    u8g2.setFont(u8g2_font_unifont_t_symbols /*u8g2_font_wqy15_t_gb2312*/);

    Serial.printf("Num nodes: %d\n", nodes.size());
    Serial.printf("Connection list:");
    u8g2.drawUTF8(0, 15, "Connection list:");
    SimpleList<uint32_t>::iterator node = nodes.begin();
    while (node != nodes.end())
    {
        Serial.printf(" %u", *node);
        sprintf(buf, "%u", *node);
        u8g2.drawUTF8(0, i * 15, buf);
        i++;
        node++;
    }

    u8g2.sendBuffer();
}

void nodeTimeAdjustedCallback(int32_t offset)
{
    Serial.printf("Adjusted time %u. Offset = %d\n", mesh.getNodeTime(), offset);
}

void setup()
{
    Serial.begin(115200);
    pinMode(2, OUTPUT);
    // mesh.setDebugMsgTypes( ERROR | MESH_STATUS | CONNECTION | SYNC | COMMUNICATION | GENERAL | MSG_TYPES | REMOTE ); // all types on
    mesh.setDebugMsgTypes(ERROR | STARTUP); // set before init() so that you can see startup messages

    mesh.init(MESH_PREFIX, MESH_PASSWORD, &userScheduler, MESH_PORT);
    mesh.onReceive(&receivedCallback);
    mesh.onNewConnection(&newConnectionCallback);
    mesh.onChangedConnections(&changedConnectionCallback);
    mesh.onNodeTimeAdjusted(&nodeTimeAdjustedCallback);
    u8g2.begin();
    u8g2.setFont(u8g2_font_unifont_t_symbols /*u8g2_font_wqy15_t_gb2312*/);

    u8g2.clearBuffer();
    u8g2.drawUTF8(0, 63, "BEGIN");
    u8g2.sendBuffer();
    //	userScheduler.addTask(taskSendMessage);
    //	taskSendMessage.enable();
}

void loop()
{
    // it will run the user scheduler as well
    mesh.update();
}
